#include "canopen_sdo.h"
#include <qDebug>


CanOpenSdo::CanOpenSdo() {
    od_table_list.push_back(odTable{0x501 ,0x3200,0x0,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.soft_version_pmu});
    od_table_list.push_back(odTable{0x502 ,0x3200,0x0,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.soft_version_wdu});
    od_table_list.push_back(odTable{0x503 ,0x3200,0x0,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.soft_version_bdu});
    od_table_list.push_back(odTable{0x504 ,0x3200,0x0,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.soft_version_hou});

    od_table_list.push_back(odTable{0x503 ,0x3080,0x0,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.param_first_save_flag});
    od_table_list.push_back(odTable{0x503 ,0x3080,0x1,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.water_suck_down_time_default});
    od_table_list.push_back(odTable{0x503 ,0x3080,0x2,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.water_suck_delay_up_time_default});
    od_table_list.push_back(odTable{0x503 ,0x3080,0x3,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.brush_push_down_time_default});
    od_table_list.push_back(odTable{0x503 ,0x3080,0x4,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.brush_push_down_time_max});
    od_table_list.push_back(odTable{0x503 ,0x3080,0x5,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.suction_delay_off_time_default});

    od_table_list.push_back(odTable{0x503 ,0x3081,0x0,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.spray_water_duty_gear1});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x1,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.spray_water_duty_gear2});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x2,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.spray_water_duty_gear3});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x3,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.left_right_brush_duty_default});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x4,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.suction_duty_default});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x5,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.brush_power_gear1});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x6,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.brush_power_gear2});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x7,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.brush_power_gear3});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x8,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.function_switch_mask});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x9,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.pump_type});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x10,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.brush_push_overload});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x11,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.clean_water_empty_disable});
    od_table_list.push_back(odTable{0x503 ,0x3081,0x12,CANOPEN_OD_RW,CANOPEN_OD_LENGTH_WORD ,(void *)&odParam.dirty_water_empty_disable});

}

bool CanOpenSdo::canOpenSdoHandle(uint32_t id , uint8_t *data)
{
    uint8_t cmd = data[0];
    uint16_t index = ((uint16_t)data[2] << 8) + data[1];
    uint8_t sub_index = data[3];

    odTable *od = getOdTableByIndex(id ,index,sub_index);
    if(od == NULL)
    {
        qDebug()<<"字典中没有该值";
        return false ;
    }
    switch(cmd)
    {
    case CANOPEN_SDO_R1:
        std::memcpy(od->data ,&data[4] ,1);
        break;
    case CANOPEN_SDO_R2:
        std::memcpy(od->data ,&data[4] ,2);
        break;
    case CANOPEN_SDO_R4:
        std::memcpy(od->data ,&data[4] ,4);
        qDebug()<<"bdu_version:"<< odParam.soft_version_bdu;
        //printParam();
        break;
    case CANOPEN_SDO_RESP://0x60
        std::memcpy(od->data ,&data[4] ,4);
        break;
    }
    return true ;

}

odTable *CanOpenSdo::getOdTableByIndex(uint32_t id ,uint32_t index, uint8_t sub_index)
{
    for(size_t i = 0; i < od_table_list.size() ;i++)
    {
        if((index == od_table_list[i].index)&&(sub_index == od_table_list[i].sub_index)&&(id == od_table_list[i].id))
        {
            return &od_table_list[i];
        }
    }
    return NULL;
}

void CanOpenSdo::printParam()
{
    qDebug()<<"param_first_save_flag:"<< odParam.param_first_save_flag;
    qDebug()<< "water_suck_down_time_default:"<< odParam.water_suck_down_time_default;
    qDebug()<<"water_suck_delay_up_time_default:"<< odParam.water_suck_delay_up_time_default;
    qDebug()<<"brush_push_down_time_default:"<< odParam.brush_push_down_time_default;
    qDebug()<<"brush_push_down_time_max:"<< odParam.brush_push_down_time_max;
    qDebug()<< "suction_delay_off_time_default:"<<odParam.suction_delay_off_time_default;
}

